/*
  COMS Testing Main Program Loop
*/

#include <msp430x16x.h>
#include <signal.h>

#include "main.h"

#include "log/log.h"
#include "timing/timing.h"
#include "recovery/recover.h"
#include "i2c/i2c.h"
#include "sd/sd.h"
#include "payload/pay.h"
#include "coms/coms.h"
#include "adcs/adcs.h"
#include "adc/adc.h"

/* "Scrubs" values; returns number of bits by which two integers differ */
int valscrub(int a, int b){
  int c,i,diff;
  
  c=a^b; //XOR a and b
  diff=0;

  for(i=0;i<16;i++) {
    diff+=c & 0x0001;
    c=c>>1;
  }

  return diff;
}

/* Clears the command variables */
void reset_command() {
  command=0;
  significance=0;
  commandToken=0;
}

/* Builds up a command bit by bit until it is complete */
int command_stack(char bit) {
  if (commandToken==1) return 0;

  command+=bit<<significance++;
  if (significance==COMMANDLENGTH) {
    significance=0;
    commandToken=1;
  }
  return 1;
}

/* Reacts to a rise on P2.0 and reads a bit from P2.1  for commands */
interrupt(PORT2_VECTOR) command_bit() {
  //command_stack((P2IN & 0x2)/2);
  //P2IE=1;
  //P2IFG=0;
}

/* Mode functions */
int doRecover(int *statusVar) {
  int i;
  //waitms(100);
  for( i=0 ; i<10000 ; i++ ) nop(); /* Temporary delay until waitms is fixed */
  statusVar[SV_TIMEOUT]++;
  //if (statusVar[SV_TIMEOUT]==0xFFFF) return MODE_AUTONOMOUS; /* Never change mode automatically */
  return MODE_RECOVER;
}

int doIdle(int *statusVar) {
  return MODE_IDLE;
}

int doOperation(int *statusVar) {
  
  statusVar[SV_ACT_SDCARD]=1; //Always use logging, for 3 system test
  
  /* Logging by whichever means are available */
  /*if (statusVar[SV_ACT_SDCARD]) {
    waitms(SHORT_PERIOD);
    address=(unsigned long int)statusVar[SV_ADDR1]<<8+(unsigned long int)statusVar[SV_ADDR2];
    address+=(unsigned long int)log_entry(address);
    statusVar[SV_ADDR1]=(int)((address & 0xFF00)>>8);
    statusVar[SV_ADDR2]=(int)(address & 0x00FF);
  } else {
    waitms(LONG_PERIOD);
    statusVar[SV_MINILOG]+=minilog_entry(&minilog[statusVar[SV_MINILOG]]);
    if (statusVar[SV_MINILOG]>=MAXMINILOG) statusVar[SV_MINILOG]=0;
  }   */

  return MODE_OPERATION;
}

int doAutonomous(int *statusVar) {
  /* Logging in RAM only */
  waitms(LONG_PERIOD);
  statusVar[SV_MINILOG]+=minilog_entry(&minilog[statusVar[SV_MINILOG]]);
  if (statusVar[SV_MINILOG]>=MAXMINILOG) statusVar[SV_MINILOG]=0;

  return MODE_AUTONOMOUS;
}

/* Entry point */
int main() {
  int statusVar[100];
  int mode, modem;
  long int i;

  unsigned char data[512];
  
  for( i=0 ; i<10000 ; i++ ) nop(); /* TESTING delay before start */
  
  mode=MODE_RECOVER;
  pos=0; /* TESTING */

  P1DIR=255;
  P1OUT=0;
  
  /* TESTING Sta */
  P5SEL = 0x00; /* Sets P5 to 3.3V I/O */
  P5OUT = 0x00; /* Sets P5 low */
  P5DIR = 0xFF; /* Sets P5 to output */
  P2SEL = 0x00; /* Sets P2 to 3.3V I/O */
  P2OUT = 0x00; /* Sets P2 low */
  P2DIR = 0xFF; /* Sets P2 to output */
  /* TESTING End */
  
  reset_command();
  eint(); //Enable interrupts

  /* Initialisations */
  sdInit();
  comsInit();
  adcInit();
  modem=modInit();
  mod_bugcount=0;
  
  /* TESTING Sta */ 
  for(i=0; i<512; i+=4) /* Fill array with test data */
  {
    data[i]=0xFF;
    data[i+1]=0xAA;
    data[i+2]=0x55;
    data[i+3]=0x00;
  }
  
  P2OUT |= 0x08; /* Sets P2.3 (H2.21) high */
  P2OUT |=0x10; /* Sets P2.4 (H2.20) high */
  P2OUT |=0x20; /* Sets P2.5 (H2.19) high */
  
  
  /* COMS test loop */
  while(1)
  {
    for( i=0 ; i<20000 ; i++ ) nop();
    P2OUT^=0x01; /* Flips P2.0 (H2.24) for TESTING */
    
    modem=modTransmit(data,512);
    
    P5OUT=modem;
    
    if(mod_bugcount>MOD_MAXBUGS) modInit(); /* If there are lots of errors reinit the modem */
    if(mod_reinit==1) modInit();
  }
  
  return 0;
  /* TESTING End */
  
  //This is the main loop. It is the core of all OBDH functionality
  while (1) {    
    /* Call the appropriate mode function */
    switch (mode) {
    case MODE_RECOVER: 
      mode=doRecover(statusVar); 
      P1OUT=0x03;
      break;

    case MODE_IDLE: 
      mode=doIdle(statusVar);
      P1OUT=0x0C; 
      break;

    case MODE_OPERATION: 
      mode=doOperation(statusVar); 
      P1OUT=0x30;
      break;

    case MODE_AUTONOMOUS: 
      mode=doAutonomous(statusVar); 
      P1OUT=0xC0;
      break;

    default:
      if (valscrub(mode, MODE_IDLE)==1) mode=MODE_IDLE;
      if (valscrub(mode, MODE_OPERATION)==1) mode=MODE_OPERATION;
      if (valscrub(mode, MODE_AUTONOMOUS)==1) mode=MODE_AUTONOMOUS;
      mode=MODE_RECOVER;
      break;
    }

    if (commandToken>0) {
      mode=MODE_OPERATION;   
      reset_command();
    }

    /* Act on any new commands passed */
    if (commandToken>0) {
      if (command & mode) statusVar[0]=command;
      if ((command & mode) == CMD_IDLE) mode=MODE_IDLE;
      if ((command & mode) == CMD_RESET) mode=MODE_RECOVER;
      if ((command & mode) == CMD_WAKE) mode=MODE_OPERATION;
      reset_command();
    }
  }
}
